#include "cameraSet.h"


CameraSet::CameraSet()
{
}


CameraSet::CameraSet( QString INPUT_FINAL_PATH_Cameras )
{
        setupCameras( INPUT_FINAL_PATH_Cameras );

        currentCameraID = 0;
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void CameraSet::setupCameras( QString INPUT_FINAL_PATH_Cameras )
{

        QFile myFile( INPUT_FINAL_PATH_Cameras );

        myFile.open(QIODevice::ReadOnly | QIODevice::Text);

        if( !myFile.isOpen() )
        {
            ErrorManager::error(3, INPUT_FINAL_PATH_Cameras);
            return;
        }

        QTextStream myTextStream(&myFile);

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        double tempDoubleToInt;

        myTextStream >> tempDoubleToInt;

        totalCameras = tempDoubleToInt;

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        openGL_Stuff.resize(totalCameras);
        openGL_Stuff.squeeze();

        for (int i=0; i<totalCameras; i++)
        {
          openGL_Stuff[i].nearPlane = 10;
          openGL_Stuff[i].farPlane  = 99999999;
        }

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        cameraMatricesStruct tempCameraStruct;

        tempCameraStruct.K      = Eigen::MatrixXd::Zero(3,3);
        tempCameraStruct.R      = Eigen::MatrixXd::Zero(3,3);
        tempCameraStruct.T      = Eigen::MatrixXd::Zero(3,1);
        tempCameraStruct.RT_3x4 = Eigen::MatrixXd::Zero(3,4);
        tempCameraStruct.RT_4x4 = Eigen::MatrixXd::Zero(4,4);
        tempCameraStruct.P_3x4  = Eigen::MatrixXd::Zero(3,4);

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        RT_4x4_custom = Eigen::MatrixXd::Identity(4,4);

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        cameras.clear();
        cameras.resize( totalCameras   );
        cameras.squeeze();
        cameras.fill(   tempCameraStruct );

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        for (int cam=0; cam<totalCameras; cam++)
        {

                for     (int i=0; i<3; i++)
                {   for (int j=0; j<3; j++)
                    {
                            myTextStream >> cameras[cam].K(i,j);
                    }
                }

                ///////////////////////////////////////////////////////////////////////////////
                ///////////////////////////////////////////////////////////////////////////////

                for     (int i=0; i<3; i++)
                {   for (int j=0; j<3; j++)
                    {
                            myTextStream >> cameras[cam].R(i,j);
                    }
                            myTextStream >> cameras[cam].T(i);
                }

                cameras[cam].RT_3x4 << cameras[cam].R, cameras[cam].T;

                cameras[cam].RT_4x4 << cameras[cam].RT_3x4(0,0), cameras[cam].RT_3x4(0,1), cameras[cam].RT_3x4(0,2), cameras[cam].RT_3x4(0,3),
                                       cameras[cam].RT_3x4(1,0), cameras[cam].RT_3x4(1,1), cameras[cam].RT_3x4(1,2), cameras[cam].RT_3x4(1,3),
                                       cameras[cam].RT_3x4(2,0), cameras[cam].RT_3x4(2,1), cameras[cam].RT_3x4(2,2), cameras[cam].RT_3x4(2,3),
                                       0,                        0,                        0,                        1;

                cameras[cam].P_3x4 =  cameras[cam].K * cameras[cam].RT_3x4;

        }

        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        myFile.close();


}




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void CameraSet::print()
{

        std::cout << totalCameras << std::endl << std::endl;

        for (int cam=0; cam<totalCameras; cam++)
        {
            std::cout << cameras[cam].K      << std::endl << std::endl;

            std::cout << cameras[cam].RT_4x4 << std::endl << std::endl << std::endl;
        }

}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



bool CameraSet::switchCamera( int newCurrCamID )
{
        /////////////////////////////////////////////////
        if (newCurrCamID >= totalCameras)   return false;
        /////////////////////////////////////////////////

        currentCameraID = newCurrCamID;

        setProjectionFrom_K();
        setModelViewFrom_RT_cameraDefault();

        ////////////
        return true;
        ////////////
}


void CameraSet::setProjectionFrom_K()
{

        double nr = openGL_Stuff[ currentCameraID ].nearPlane;
        double fr = openGL_Stuff[ currentCameraID ].farPlane;

        // CARMINE / KINECT
        double w  = 640;
        double h  = 480;
        double fx = cameras[ currentCameraID ].K(0,0);
        double fy = cameras[ currentCameraID ].K(1,1);
        double px = cameras[ currentCameraID ].K(0,2);
        double py = cameras[ currentCameraID ].K(1,2);

                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy.clear();
                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy.resize(4);
                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy[0] = fx;
                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy[1] = fy;
                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy[2] = px;
                    cameras[ currentCameraID ].intrinsicsVec_fx_fy_cx_cy[3] = py;

        Eigen::MatrixXd matK_columnMajor(4,4);   //myproj

        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        //matK_columnMajor     <<     (2*fx/w),       0,           (2*px/w-1),           0,
        //                            0,             -(2*fy/h),   -(2*py/h-1),           0,
        //                            0,              0,          +(fr+nr)/(fr-nr),     -(2*fr*nr/(fr-nr)),
        //                            0,              0,           (1),                  0;
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        double dx = px - w / 2;
        double dy = py - h / 2;
        double x  =  2.0f * fx / w;
        double y  = -2.0f * fy / h;
        double a  =  2.0f * dx / w;
        double b  = -2.0f * (dy + 1.0f) / h;
        double c  = (fr + nr) / (fr - nr);
        double d  = - nr * (1.0f + c);

        matK_columnMajor <<  x,  0,  a,  0,
                             0,  y,  b,  0,
                             0,  0,  c,  d,
                             0,  0,  1,  0;

        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glLoadMatrixd(matK_columnMajor.data());

}



void CameraSet::setModelViewFrom_RT_cameraDefault()
{
        RT_4x4_custom = cameras[ currentCameraID ].RT_4x4;

        setModelViewFrom_RT_customMat_4_4();
}


void CameraSet::setModelViewFrom_RT_customMat_4_4()
{
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        glMultMatrixd( RT_4x4_custom.data() );
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
